#include <occmesh/api/lscm.h>
#include <occmesh/api/timing.h>

const float MIN_DEFAULT = FLT_MAX;
const float MAX_DEFAULT = -FLT_MAX;
const float THRESHOLD = 0.0000001f;

LSCMBuilder::LSCMBuilder(Mesh *mesh, FeatureMetric *metric) : m_mesh(mesh), m_metric(metric)
{
    m_mesh->add_property(m_vertexStates);
    m_mesh->add_property(m_faceId);
}

LSCMBuilder::~LSCMBuilder()
{
    m_mesh->remove_property(m_vertexStates);
    m_mesh->remove_property(m_faceId);
}

std::vector<Chart> LSCMBuilder::Build()
{
    std::vector<Chart> charts;

    // 根据是否传入度量判定是否进行特征检测
    if (m_metric != nullptr)
    {
        TIMER_START(FEATURE);
        SODFeatureMetric metric(15);
        FeatureBuilder fBuilder(m_mesh, &metric, 10, 20);
        TIMER_END(FEATURE);

        TIMER_START(CHART);
        ChartBuilder cBuilder(m_mesh, fBuilder.Build());
        charts = cBuilder.Build();
        TIMER_END(CHART);

        if (!cBuilder.Validate())
            std::cout << "Charts not validate." << std::endl;
    }

    // 如果特征检测失败，就直接将整个网格作为图表
    if (charts.empty())
    {
        charts.push_back(Chart());
        Chart &chart = charts.back();

        auto &vertices = chart.vertices();
        for (auto &face : m_mesh->faces())
            chart.add(face, 0);

        for (auto &v : m_mesh->vertices())
            vertices.push_back(v);
    }

    // 依次构建图表
    for (auto &chart : charts)
    {
        // 初始化标记，获得活动顶点的数量
        int act = initializeIndex(chart);

        // M 的行数等于三角形数，列数等于顶点数，是系数矩阵
        // 由于虚实分离，得到 A 矩阵的规模是 2 倍行数
        const auto numRows = chart.faces().size() * 2;
        SparseMatrix A(numRows, act * 2);
        SparseMatrix fA(numRows, (chart.vertices().size() - act) * 2);
        MatrixXx1 fx(fA.cols());

        // 填充系数，获得基本矩阵
        initializeSystem(chart, A, fA, fx);

        // Ax = b, b = -Mp * Up
        MatrixXx1 e = -fA * fx;

        // 稀疏矩阵最小二乘共轭梯度求解器
        Eigen::LeastSquaresConjugateGradient<SparseMatrix> solver;
        solver.setTolerance(THRESHOLD);

        // 设置最大迭代次数，默认是列数 * 2
        solver.setMaxIterations(A.cols());
        solver.compute(A);

        // 存储 UV 参数化
        MatrixXx1 x = solver.solve(e);
        for (const auto &vertex : chart.vertices())
        {
            // 获得顶点信息
            auto &state = m_mesh->property(m_vertexStates, vertex);

            // 固定顶点已经有参数坐标，只需要考虑活动顶点
            auto uv = Mesh::TexCoord2D();
            if (state.fixed)
                uv = m_mesh->texcoord2D(vertex);
            else
            {
                uv = Mesh::TexCoord2D(x[state.id], x[state.id + 1]);
                m_mesh->set_texcoord2D(vertex, uv);
            }
        }
    }

    return charts;
}

Mesh::Point LSCMBuilder::OBB(const VertexList &vertices)
{
    int n = vertices.size();

    // 坐标分离为 3 个随机变量
    Eigen::VectorXd X(n), Y(n), Z(n);
    for (int i = 0; i < n; i++)
    {
        auto &p = m_mesh->point(vertices[i]);

        X[i] = p[0];
        Y[i] = p[1];
        Z[i] = p[2];
    }

    // 计算期望点
    double E[] = {0, 0, 0};
    E[0] = X.sum() / n;
    E[1] = Y.sum() / n;
    E[2] = Z.sum() / n;

    std::cout << "N:" << n << std::endl;
    std::cout << "E:" << E[0] << " " << E[1] << " " << E[2] << std::endl;

    // 计算协方差矩阵
    Eigen::MatrixXd B(3, 3);
    B.setZero();
    for (int i = 0; i < n; i++)
    {
        B(0, 0) += X[i] * X[i];
        B(0, 1) += X[i] * Y[i];
        B(0, 2) += X[i] * Z[i];

        B(1, 0) += Y[i] * X[i];
        B(1, 1) += Y[i] * Y[i];
        B(1, 2) += Y[i] * Z[i];

        B(2, 0) += Z[i] * X[i];
        B(2, 1) += Z[i] * Y[i];
        B(2, 2) += Z[i] * Z[i];
    }
    B = B / n;

    // 计算 Xi_i 和 Xi_j 的协方差
    for (int i = 0; i < 3; i++)
        for (int j = 0; j < 3; j++)
            B(i, j) -= E[i] * E[j];

    std::cout << "B:\n" << B << std::endl;

    // 特征分解
    Eigen::EigenSolver<Eigen::MatrixXd> es(B);

    // 获得特征值，取实部
    Eigen::VectorXcd cval = es.eigenvalues();
    Eigen::VectorXd val(cval.size());
    for (int i = 0; i < val.size(); i++)
        val[i] = cval[i].real();

    // 获得特征向量的矩阵，取实部
    Eigen::MatrixXcd cV = es.eigenvectors();
    Eigen::MatrixXd V(cV.rows(), cV.cols());
    for (int i = 0; i < V.rows(); i++)
        for (int j = 0; j < V.cols(); j++)
            V(i, j) = cV(i, j).real();

    std::cout << "eigens:\n" << cval << std::endl;
    std::cout << "eigenvectors:\n" << cV << std::endl;

    // 取出最大主方向
    int maxId = 0;
    double maxVal = MAX_DEFAULT;
    for (int i = 0; i < 3; i++)
    {
        if (val[i] > maxVal)
        {
            maxId = i;
            maxVal = val[i];
        }
    }

    return Mesh::Point(V(0, maxId), V(1, maxId), V(2, maxId)).normalized();
}

int LSCMBuilder::initializeIndex(const Chart &chart)
{
    // 计算主方向，获得锚定点
    auto &vertices = chart.vertices();

    // 第一个图表使用最远端点作为锚定点
    Mesh::Point mainDir = OBB(vertices);

    double minProj = MIN_DEFAULT;
    double maxProj = MAX_DEFAULT;
    Mesh::VertexHandle h1, h2;
    for (auto &v : vertices)
    {
        auto &p = m_mesh->point(v);
        double proj = p | mainDir;

        if (proj < minProj)
        {
            h1 = v;
            minProj = proj;
        }

        if (proj > maxProj)
        {
            h2 = v;
            maxProj = proj;
        }
    }

    std::cout << "Dir: " << mainDir << std::endl;
    std::cout << "min: " << minProj << std::endl;
    std::cout << "max: " << maxProj << std::endl;

    // 标记活动顶点序号
    int act = 0;
    for (auto &v : vertices)
    {
        auto &state = m_mesh->property(m_vertexStates, v);
        if (v != h1 && v != h2)
        {
            state.id = act * 2;
            state.fixed = false;
            act++;
        }
    }

    // 标记锚点序号
    m_mesh->property(m_vertexStates, h1) = {true, 0};
    m_mesh->property(m_vertexStates, h2) = {true, 2};

    // 记录锚定点的基准参数坐标
    m_mesh->set_texcoord2D(h1, {0, 0});
    m_mesh->set_texcoord2D(h2, {1, 1});

    // 标记面对应的序号
    auto &faces = chart.faces();
    for (unsigned int i = 0; i < faces.size(); i++)
        m_mesh->property(m_faceId, faces[i]) = i * 2;

    return act;
}

void LSCMBuilder::initializeSystem(const Chart &chart, SparseMatrix &A, SparseMatrix &fA, MatrixXx1 &fx)
{
    // 用来初始化稀疏矩阵
    TripletList a, fa;

    // 处理每个面
    for (const auto &face : chart.faces())
    {
        // 获得面对应的 id，也就是它对应的行标。实数行标和虚数行标
        const auto realRow = m_mesh->property(m_faceId, face);
        const auto imRow = realRow + 1;

        // 计算三角面的局部坐标值存在 pv 中
        Mesh::Point pv[3];
        Mesh::VertexHandle hv[3];
        projectFace(face, pv, hv);

        const auto pv01 = pv[1] - pv[0];
        const auto pv02 = pv[2] - pv[0];

        // 设置系数的函数
        auto setCoefficient = [&](int row, const Mesh::VertexHandle &vertex, double u, double v) {
            // 区分固定点和活动点
            auto &state = m_mesh->property(m_vertexStates, vertex);
            auto &target = state.fixed ? fa : a;

            // 填充相邻两列，分别对应实部和虚部
            target.emplace_back(Triplet{row, state.id, u});
            target.emplace_back(Triplet{row, state.id + 1, v});

            // 固定点还需要设置
            if (state.fixed)
            {
                auto &tex = m_mesh->texcoord2D(vertex);
                fx[state.id] = tex[0];
                fx[state.id + 1] = tex[1];
            }
        };

        // 填充 a 系数，a 是 Triplet 存放稀疏格式 (i,j,value)
        // Real and Imaginary
        // 按照下面这种格式填充
        // Mf1 -Mf2		=> 		-pv01[0] + pv02[0] 		pv01[1] - pv02[1]
        // Mf2 Mf1		=> 		-pv01[1] + pv02[1] 		-pv01[0] + pv02[0]
        setCoefficient(realRow, hv[0], -pv01[0] + pv02[0], pv01[1] - pv02[1]);
        setCoefficient(imRow, hv[0], -pv01[1] + pv02[1], -pv01[0] + pv02[0]);

        setCoefficient(realRow, hv[1], -pv02[0], pv02[1]);
        setCoefficient(imRow, hv[1], -pv02[1], -pv02[0]);

        setCoefficient(realRow, hv[2], pv01[0], 0);
        setCoefficient(imRow, hv[2], 0, pv01[0]);
    }

    // 获得稀疏矩阵
    fA.setFromTriplets(fa.begin(), fa.end());
    fA.makeCompressed();

    A.setFromTriplets(a.begin(), a.end());
    A.makeCompressed();
}

void LSCMBuilder::projectFace(const Mesh::FaceHandle &face, Mesh::Point *pv, Mesh::VertexHandle *hv)
{
    Mesh::Point v[3];

    // 记录面上的所有顶点和 (u,v,h)信息
    auto i = 0;
    auto v_it = m_mesh->fv_begin(face), v_end = m_mesh->fv_end(face);
    for (; v_it != v_end; v_it++, i++)
    {
        v[i] = m_mesh->point(*v_it);
        hv[i] = *v_it;
    }

    const auto v10 = v[1] - v[0];
    const auto v10Length = v10.length();
    const auto v20 = v[2] - v[0];

    const auto x = v10 / v10Length;

    // 这里 % 是叉积！！！！！！！！！
    const auto z = (x % v20).normalized();
    const auto y = z % x;

    // 记录三角形顶点的坐标，注意 | 是点乘！！！！！！！！！！！
    pv[0] = Mesh::Point(0, 0, 0);
    pv[1] = Mesh::Point(v10Length, 0, 0);
    pv[2] = Mesh::Point(v20 | x, v20 | y, 0);
}